#include <QtGui>
#include <iostream>

#include <string>
#include <QString>
#include <iostream>
#include <sstream>
#include <cmath>

#include <QTimer>

#define TIMER 20
#define ROBOTID 0

#define GUI 1

#define V1 6
#define V2 5
#define V3 1
#define FL 4
#define FR 2
#define LA 3

#include "gui.h"

Gui::Gui(QMainWindow *parent) : QMainWindow(parent)
{
  if(GUI)
    ui.setupUi(this);
  
  timeAck = 0;
             
  phys = new Physics();
  motor = new Motor();
  eth = new Ethernet();
  //servo = new Servo();

  timerId = startTimer(TIMER);
	
	connected = false;
	
	forward1 = 0;
	forward2 = 0;
	lateral = 0;
	
	joyFor = 0;
	joyLat = 0;
	joySpin = 0;
	depth = 0;
	
	thrust1 = 0;
	thrust2 = 0;
	thrust3 = 0;
	
	up1 = 0;
	up2 = 0;
	up3 = 0;
	
	useCorrection = false;
  

}
 
void Gui::timerEvent(QTimerEvent *event)
{
  if(eth->hasDatagrams())
    eth->checkPendingDatagrams();
    
  timeAck++;
  
  if(timeAck == 20)
  {
//    std::cout << "Entering motor checking" << std::endl;
    timeAck = 0;
    for(int i = 1; i <= 6; i++)
    {
      motor->getErrors(i);
      eth->setMotor(i,
                    motor->getSpeed(i),
                    motor->getTemperature(i),
                    motor->getVoltage(i));
    }
    
    eth->writeMotor();
  //} previous bracket location
  
    if(eth->isConnected())
      eth->writeUdp();
    else
      eth->sendConnect();
  }
  
  phys->run();

  joyFor = eth->getForward();
  joyLat = eth->getLateral();
  joySpin = eth->getSpin();
  depth = eth->getDepth();
  u1j = eth->getup(1);
  u2j = eth->getup(2);
  u3j = eth->getup(3);
  
  /*if(eth->getServo() == 1)
  {
    servo->incUp();
  }
  else if(eth->getServo() == -1)
  {
    servo->incDown();
  }*/
  
  if(GUI)
  {
    ui.connectedLed->setValue(eth->isConnected());
    
    ui.yawLabel   ->setText(QString("%1").arg(phys->getChannel(0)));  
    ui.pitchLabel ->setText(QString("%1").arg(phys->getChannel(1)));  
    ui.rollLabel  ->setText(QString("%1").arg(phys->getChannel(2)));  
    ui.yawRLabel  ->setText(QString("%1").arg(phys->getChannel(3)));  
    ui.pitchRLabel->setText(QString("%1").arg(phys->getChannel(4)));  
    ui.rollRLabel ->setText(QString("%1").arg(phys->getChannel(5)));  
    ui.magZLabel  ->setText(QString("%1").arg(phys->getChannel(6)));  
    ui.magYLabel  ->setText(QString("%1").arg(phys->getChannel(7)));  
    ui.magXLabel  ->setText(QString("%1").arg(phys->getChannel(8)));  
    ui.gyroZLabel ->setText(QString("%1").arg(phys->getChannel(9)));  
    ui.gyroYLabel ->setText(QString("%1").arg(phys->getChannel(10)));  
    ui.gyroXLabel ->setText(QString("%1").arg(phys->getChannel(11)));  
    ui.accelZLabel->setText(QString("%1").arg(phys->getChannel(12)));  
    ui.accelYLabel->setText(QString("%1").arg(phys->getChannel(13)));  
    ui.accelXLabel->setText(QString("%1").arg(phys->getChannel(14)));  
  }
  thrust1 = phys->getThrusterPositions(0);
  thrust2 = phys->getThrusterPositions(1);
  thrust3 = phys->getThrusterPositions(2);
  
  setThrusters();
   
  if(GUI) 
    repaint();			 //paint the window
  
  eth->setPhysVals(phys->getChannel(0),phys->getChannel(1),phys->getChannel(2));
  
}

void Gui::setThrusters()
{
  calcThrust();
  /*
  up1 /= 3;
  up2 /= 3;
  up3 /= 3;
  */
  if(up1 < 0)
  {
    if(GUI)
    {
      ui.up1Set->setValue(abs(up1));
      ui.up1Set->setInvertedAppearance(true);
    }
    
    motor->motorReverse(V1,abs(up1)/100.);
  }
  else if(up1 > 0)
  {
    if(GUI)
    {
      ui.up1Set->setValue(up1);
      ui.up1Set->setInvertedAppearance(false);
    }
        
    motor->motorForward(V1,up1/100.);
  }
  else
  { 
    if(GUI)
    {
      ui.up1Set->setValue(0);   
      ui.up1Set->setInvertedAppearance(false);
    }
    
    motor->motorForward(V1,0);
  }
  
  if(up2 < 0)
  {
    if(GUI)
    {
      ui.up2Set->setValue(abs(up2));
      ui.up2Set->setInvertedAppearance(true);
    }
    motor->motorReverse(V2,abs(up2)/100.);
  }
  else if(up2 > 0)
  {
    if(GUI)
    {
      ui.up2Set->setValue(up2);
      ui.up2Set->setInvertedAppearance(false);
    }    
    motor->motorForward(V2,up2/100.);
  }
  else
  { 
    if(GUI)
    {
      ui.up2Set->setValue(0);   
      ui.up2Set->setInvertedAppearance(false);
    }
    motor->motorForward(V2,0);
  }
  
  if(up3 < 0)
  {
    if(GUI)
    {
      ui.up3Set->setValue(abs(up3));
      ui.up3Set->setInvertedAppearance(true);
    }
    motor->motorForward(V3,abs(up3)/100.);
  }
  else if(up3 > 0)
  {
    if(GUI)
    {
      ui.up3Set->setValue(up3);
      ui.up3Set->setInvertedAppearance(false);
    }
    motor->motorReverse(V3,up3/100.);
  }
  else
  { 
    if(GUI)
    {
      ui.up3Set->setValue(0);   
      ui.up3Set->setInvertedAppearance(false);
    }
    motor->motorForward(V3,0);
  }
  
  if(forward1 < 0)
  {
    if(GUI)
    {
      ui.forward1Set->setValue(forward1*-1);
      ui.forward1Set->setInvertedAppearance(true);
    }
    motor->motorForward(FL,forward1*-1/100.);
  }
  else
  {
    if(GUI)
    {
      ui.forward1Set->setValue(forward1);
      ui.forward1Set->setInvertedAppearance(false);
    }
    motor->motorReverse(FL,forward1/100.);
  }
  
  if(forward2 < 0)
  {
    if(GUI)
    {
      ui.forward2Set->setValue(forward2*-1);
      ui.forward2Set->setInvertedAppearance(true);
    }
    motor->motorReverse(FR,forward2*-1/100.);
  }
  else
  {
    if(GUI)
    {
      ui.forward2Set->setValue(forward2);
      ui.forward2Set->setInvertedAppearance(false);
    }
    motor->motorForward(FR,forward2/100.);
  }
  
  if(lateral < 0)
  {
    if(GUI)
    {
      ui.lateralSet->setValue(lateral*-1);
      ui.lateralSet->setInvertedAppearance(true);
    }
    motor->motorForward(LA,lateral*-1/100.);
  }
  else
  {
    if(GUI)
    {
      ui.lateralSet->setValue(lateral);
      ui.lateralSet->setInvertedAppearance(false);
    }
    motor->motorReverse(LA,lateral/100.);
  }
}

void Gui::calcThrust()
{
  forward1 = joyFor;
  forward2 = joyFor;
  lateral = joyLat;
  
  forward1 += joySpin;
  forward2 -= joySpin;
  
  if(forward1 > 100)
  {
    int excess = forward1-100;
    forward1 = 100;
    forward2 -= excess;
  }
  else if(forward1 < -100)
  {
    int excess = forward1+100;
    forward1 = -100;
    forward2 += excess;
  }
  
  if(forward2 > 100)
  {
    int excess = forward2-100;
    forward2 = 100;
    forward1 -= excess;
  }
  else if(forward2 < -100)
  {
    int excess = forward2+100;
    forward2 = -100;
    forward1 += excess;
  }
  
  if(eth->getImuControl())
  {
    if(abs(thrust1) > .5)
    {
      up1 = (int)(thrust1/2.*100);
//      std::cout << "Thrust1:" << thrust1 << " up1:" << up1 << std::endl;
    }
    else
    {
      up1 = 0;
    }
  
    if(abs(thrust2) > .5)
    {
      up2 = (int)(thrust2/2.*100);
//      std::cout << "Thrust2:" << thrust2 << " up2:" << up2 << std::endl;
    }
    else
    {
      up2 = 0;
    }
  
    if(abs(thrust3) > .5)
    {
      up3 = (int)(thrust3/2.*100);
//      std::cout << "Thrust3:" << thrust3 << " up3:" << up3 << std::endl;
    }
    else
    {
      up3 = 0;
    }
  }
  
  up1 += (int)(depth);
  up2 += (int)depth;
  up3 += (int)depth;
  
  up1 += u1j*25;
  up2 += u2j*25;
  up3 += u3j*25;
  
  if(abs(up1) > 100)
  {
    double excess = abs(up1)/100.;
    up1 = (int)(up1/excess);
    up2 = (int)(up2/excess);
    up3 = (int)(up3/excess);
  }
  
  if(abs(up2) > 100)
  {
    double excess = abs(up2)/100.;
    up1 = (int)(up1/excess);
    up2 = (int)(up2/excess);
    up3 = (int)(up3/excess);
  }
  
  if(abs(up3) > 100)
  {
    double excess = abs(up3)/100.;
    up1 = (int)(up1/excess);
    up2 = (int)(up2/excess);
    up3 = (int)(up3/excess);
  }
  
}
